/*
 * Camera subsystem. The subsystem consists of a camera and 2 servos which
 * move the camera.  One servo controls left-right.  The other controls 
 * up/down
 */
package edu.wpi.first.wpilibj.templates.subsystems;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.templates.commands.MoveCamWithJoystick;


/**
 *
 * @author anne
 */
public class Cam extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    private static final double CAM_MIDPT_X= 0.5;
    private static final double CAM_MIDPT_Y= 0.5;
    
    AxisCamera m_cam;
    Servo m_camX;
    Servo m_camY;
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        /**
         * Set the default command for the drive train. The default
         * command is the command (if any) that will execute when there is
         * no other command running on the subsystem
         */
        setDefaultCommand(new MoveCamWithJoystick());
    }
    
    public Cam() {
        m_camX= new Servo(RobotMap.CAM_SERVO_X);
        m_camY= new Servo(RobotMap.CAM_SERVO_Y);
        
        /**
         * Set camera servos to mid point
         */
        m_camX.set(CAM_MIDPT_X);
        m_camY.set(CAM_MIDPT_Y);
        
        /**
         * Get camera object 
         */
        m_cam= AxisCamera.getInstance(RobotMap.CAMERA_IP_ADDRESS);
    }
    
    /**
     * This method moves the camera servos to correspond to the current joystick position
     * @param stick Joystick controlling movement
     */
    public void moveCamWithJoystick(Joystick stick) {
        m_camX.set(stick.getX());
        m_camY.set(stick.getY());
    }
}
